/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;
import edu.wpi.first.wpilibj.templates.commandGroups.DriveInASquare;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {

    Command autonomousCommand; // Command to be executed in autonomous mode

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        /* 
         * Instantiate the command used for the autonomous period
         * Upon entry to autonomous mode, set up to execute the command group 
         * to drive the robot in a square
         */
        autonomousCommand= new DriveInASquare();
        
        /*
         * Initialize all subsystems.  This will create the operator
         * interface 
         */
        CommandBase.init();
    }

    /**
     * This method will be called at the start of autonomous mode.
     * It can be used to start executing commands for autonomous mode
     */
    public void autonomousInit() {
        /*
         * Schedule the autonomous command to start 
         * Note: We have mapped the autonomous command to a command group
         * which drives the robot in a square. Thus, this should schedule
         * the robot to start driving the robot in a square.
         */
        autonomousCommand.start();
    }

    /**
     * This function is called periodically during autonomous mode.  It will
     * run any commands that are still scheduled for autonomous mode
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }

    /**
     *  This method is called when tele-op (operator controlled) mode is started.
     *  (Presumably during a competition the robot was previously in autonomous mode)
     */
    public void teleopInit() {
	// This makes sure that the autonomous stops running when
        // teleop starts running. If you want the autonomous to 
        // continue until interrupted by another command, remove
        // this line or comment it out.
        autonomousCommand.cancel();
    }

    /**
     * This function is called periodically during operator control.  Any 
     * commands that are still scheduled for tele-op mode are run. If there are 
     * no commands currently scheduled, each subsystem will execute its default
     * command (if one has been set up).  
     * Note: We have set up one subsystem for the drive train (chassis). In that
     * subsystem we set the default command to driveWithJoystick. Therefore, 
     * if no other commands are scheduled, the robot will respond to the operator's
     * joystick control. However, since we mapped a trigger to the joystick's
     * top button, if the operator presses the top button on the joystick, the
     * robot will stop responding to the joystick and instead execute the command
     * group that was mapped to execute when the button is pressed (DriveInASquare).
     * After that command group completes, the robot will go back to executing
     * its default command.
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
    }
}

